#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S3, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     DriveFL,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     DriveBL,       tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     LiftL,         tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     DriveFR,       tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     DriveBR,       tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_1,     LiftR,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S3_C1_2,     Roller,        tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S3_C2_1,    TubeG,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_2,    LDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_3,    RDoor,                tServoStandard)
#pragma config(Servo,  srvo_S3_C2_4,    SensorServo,          tServoStandard)
#pragma config(Servo,  srvo_S3_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S3_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"


task main()
{



//Defining all necessary Variables.

bool LiftUp;
bool TubeGUp;
bool DoorBtnPress;


//Setting all variables to their necessary values.

TubeGUp = false;
LiftUp = false;
DoorBtnPress = false;

nMotorEncoder[Roller] = 0;
nMotorEncoder[LiftR] = 0;
nMotorEncoder[LiftL] = 0;

servo[LDoor] = 0;
servo[RDoor] = 256;
servo[TubeG] = 186;
servo[SensorServo] = 20;

while(true)
	{

	getJoystickSettings(joystick);

	//The following code is for the Drivetrain.  It has been coded that all drive motion is controlled by Joystick 1.

		//Left Drive
if((nMotorEncoder[LiftR] < 850 || TubeGUp == true) || (joy2Btn(9) == 1 && joy2Btn(10) == 1))

{

	if(joystick.joy1_y1 > 10 || joystick.joy1_y1 < -10)
		{

		motor[DriveFL] = joystick.joy1_y1;
		motor[DriveBL] = joystick.joy1_y1;

		}

	else
		{

		motor[DriveFL] = 0;
		motor[DriveBL] = 0;

		}



		//Right Drive

	if(joystick.joy1_y2 > 10 || joystick.joy1_y2 < -10)
		{

		motor[DriveFR] = joystick.joy1_y2;
		motor[DriveBR] = joystick.joy1_y2;

		}

	else
		{

		motor[DriveFR] = 0;
		motor[DriveBR] = 0;

		}

}

else

{

motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;

}

	//The following code is for the roller.  It has been coded so that the Roller is controlled by Joystick 2.

			//Normal Operation

	if(LiftUp == false && TubeGUp == false)
	{

		if(joy2Btn(8) == 1 && joy2Btn(7) == 0)

		{

		motor[Roller] = 40;

		}



		//Reverse Operation.  This is in case we accidentally pick up more than five balls.

		if(joy2Btn(7) == 1 && joy2Btn(8) == 0)

		{

		motor[Roller] = -100;

		}

		//If both triggers are pressed, stop the motor.


		if(joy2Btn(5) == 1)
		{

		motor[Roller] = 0;

		}

	}

	//If the lift is up for any reason, ball dispensing or tube grabbing, the roller MUST stay off.

	else
	{

	motor[Roller] = 0;

	}

		//The following code is for the tube-grabber.  It has been coded so that the tube-grabber is controlled by Joystick 1.

	if(joy1Btn(4) == 1)
		{

		TubeGUp = true;

		motor[Roller] = 0;

		motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


		while(nMotorEncoder[LiftR] < 800)
			{

			motor[LiftR] = 100;
			motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

		wait1Msec(350);

		servo[TubeG] = 255;


		}

	if(joy1Btn(2) == 1)
		{

		motor[Roller] = 0;

		motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


		servo[TubeG] = 186;

		wait1Msec(350);

		while (nMotorEncoder[LiftR] > 400)

			{

					motor[LiftR] = 1;
					motor[LiftL] = 1;

			}

		while(nMotorEncoder[LiftR] >= 50)

			{

			motor[LiftR] = -1;
			motor[LiftL] = -1;

			}

		motor[LiftR] = 0;
		motor[LiftL] = 0;

		nMotorEncoder[LiftR] = 0;
		nMotorEncoder[LiftL] = 0;

		TubeGUp = false;

		}




//		//The following code is to control the lift.


if(joy2Btn(1) == 1)
{

	motor[Roller] = 0;

	motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] < 890)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

		LiftUp = true;

}



if(joy2Btn(2) == 1)
{

	motor[Roller] = 0;

	motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] < 1675)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

		LiftUp = true;

}



if(joy2Btn(3) == 1)
{

	motor[Roller] = 0;

	motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] < 2525)
		{

		motor[LiftR] = 100;
		motor[LiftL] = 100;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

		LiftUp = true;

}



//if(joy2Btn(4) == 1)
//{

//	motor[Roller] = 0;

//	while(nMotorEncoder[LiftR] < 3300)
//		{

//		motor[LiftR] = 100;
//		motor[LiftL] = 100;

//		}

//		motor[LiftR] = 15;
//		motor[LiftL] = 15;

//		LiftUp = true;

//}


if(joy2Btn(1) == 1 && joy2Btn(10) == 1)
{

	motor[Roller] = 0;

	motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] > 890)
		{

		motor[LiftR] = 1;
		motor[LiftL] = 1;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

		LiftUp = true;

}



if(joy2Btn(2) == 1 && joy2Btn(10) == 1)
{

	motor[Roller] = 0;

	motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] > 1675)
		{

		motor[LiftR] = 1;
		motor[LiftL] = 1;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

		LiftUp = true;

}



if(joy2Btn(3) == 1 && joy2Btn(10) == 1)
{

	motor[Roller] = 0;

	motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


	while(nMotorEncoder[LiftR] > 2525)
		{

		motor[LiftR] = 1;
		motor[LiftL] = 1;

		}

		motor[LiftR] = 15;
		motor[LiftL] = 15;

		LiftUp = true;

}



			//Zeroes the lift.
	if(joy2Btn(6) == 1 && TubeGUp == false)
		{

		motor[Roller] = 0;

		motor[DriveFR] = 0;
motor[DriveFL] = 0;
motor[DriveBR] = 0;
motor[DriveBL] = 0;


		while (nMotorEncoder[LiftR] > 800)

			{

			motor[Roller] = 0;

			motor[LiftR] = 1;

					motor[LiftL] = 1;

			}

		while(nMotorEncoder[LiftR] >= 50)

			{

			motor[Roller] = 0;

			motor[LiftR] = -1;
			motor[LiftL] = -1;

			}

		motor[Roller] = 0;

		motor[LiftR] = 0;
		motor[LiftL] = 0;

		nMotorEncoder[LiftR] = 0;
		nMotorEncoder[LiftL] = 0;


		LiftUp = false;

		}



		//The following code is to control the doors.

	if(LiftUp == true && TubeGUp == false)
		{

		if(joy1Btn(8) == 1)
		{

		DoorBtnPress = true;

		}

	while(DoorBtnPress == true)
		{

		wait1Msec(50);

		servo[RDoor] = 172;
		wait1Msec(2000);

		servo[LDoor] = 65;
		wait1Msec(2000);

		servo[RDoor] = 256;
		servo[LDoor] = 0;

		wait1Msec(500);
		DoorBtnPress = false;

		}

	}

}

}
